/*
 * island.c
 *
 *  Created on: 2024年6月28日
 *      Author: Kenobi
 */

#include "island.h"

Image_Change_Struct Image_Change; // 单调性改变，连续性数撕裂点结构体
Island_Struct Island;             // 环岛结构体

/*-------------------------------------------------------------------------------------------------------------------
  @brief     环岛检测
  @param     null
  @return    null
  Sample     Island_Detect(void);
  @note      利用四个拐点判别函数，单调性改变函，连续性数撕裂点，分为8步
-------------------------------------------------------------------------------------------------------------------*/
void Island_Detect()
{
    memset(&Image_Change, 0, sizeof Image_Change);
    if(Barrier.Barrier_Flag)return;

    Image_Change.continuity_change_left_flag = Continuity_Change_Left(LCDH - 1 - 5, 10); // 连续性判断
    Image_Change.continuity_change_right_flag = Continuity_Change_Right(LCDH - 1 - 5, 10);
    Image_Change.monotonicity_change_right_flag = Monotonicity_Change_Right(LCDH - 1 - 10, 10);
    Image_Change.monotonicity_change_left_flag = Monotonicity_Change_Left(LCDH - 1 - 10, 10);

    if (Cross.Cross_Flag == 0 && Island.Island_State == 0)
    {
        Image_Change.continuity_change_left_flag = Continuity_Change_Left(LCDH - 1 - 5, 10); // 连续性判断
        Image_Change.continuity_change_right_flag = Continuity_Change_Right(LCDH - 1 - 5, 10);
        if (Island.Left_Island_Flag == 0) // 左环
        {
            if (Image_Change.monotonicity_change_right_flag == 0 &&                            // 右边是单调的
                Image_Change.continuity_change_left_flag != 0 &&                               // 左边是不连续的
                Image_Change.continuity_change_right_flag == 0 &&                              // 左环岛右边是连续的
                Lost.Left_Num >= ISLAND_LOST_MIN &&                                            // 左边丢线很多
                Lost.Left_Num <= ISLAND_LOST_MAX &&                                            // 也不能全丢了
                Lost.Right_Num <= ISLAND_OTHER_LOST_MAX &&                                     // 右边丢线较少
                LonWCol.Search_Stop_Line >= ISLAND_SEARCH_STOP_MIN &&                          // 搜索截止行看到很远
                Lost.Start_Left >= ISLAND_START_Min && Lost.Start_Right >= ISLAND_START_Min && // 边界起始点靠下
                Lost.All_Num <= ISLAND_All_LOST_MAX)                                           // 双边丢线少
            {
                Island.Left_Down_Guai[0] = Find_Left_Down_Point(LCDH - 1, 10); // 找左下角点
                if (Island.Left_Down_Guai[0] >= ISLAND_FOOT_THRESHOLD_MIN)     // 条件1很松，在这里判断拐点，位置不对，则是误判，跳出
                {
                    Island.Island_State = 1;
                    Island.Left_Island_Flag = 1;
                }
                else // 误判，归零
                {
                    Island.Island_State = 0;
                    Island.Left_Island_Flag = 0;
                }
            }
        }
        if(Island.Right_Island_Flag==0)//右环
        {
            if(Image_Change.monotonicity_change_left_flag==0&&
               Image_Change.continuity_change_left_flag==0&& //右环岛左边是连续的
               Image_Change.continuity_change_right_flag!=1&& //右边是不连续的
               Lost.Right_Num>=ISLAND_LOST_MIN&&           //右丢线多
               Lost.Right_Num<=ISLAND_LOST_MAX&&           //右丢线不能太多
               Lost.Left_Num<=ISLAND_OTHER_LOST_MAX&&            //左丢线少
               LonWCol.Search_Stop_Line>=ISLAND_SEARCH_STOP_MIN&& //搜索截止行看到很远
               Lost.Start_Left>=ISLAND_START_Min&&Lost.Start_Right>=ISLAND_START_Min&& //边界起始点靠下
               Lost.All_Num<=ISLAND_All_LOST_MAX)
            {
                Island.Right_Down_Guai[0]=Find_Right_Down_Point(LCDH-1,10);//右下点
                if(Island.Right_Down_Guai[0]>=ISLAND_FOOT_THRESHOLD_MIN)//条件1很松，在这里加判拐点，位置不对，则是误判，跳出
                {
                    Island.Island_State=1;
                    Island.Right_Island_Flag=1;
                }
                else
                {
                    Island.Island_State=0;
                    Island.Right_Island_Flag=0;
                }
            }
        }
    }
    if (Island.Left_Island_Flag == 1) // 1状态下拐点还在，没丢线
    {
        if (Island.Island_State == 1)
        {
            Image_Change.monotonicity_change_line[0] = Monotonicity_Change_Left(30, 5); // 寻找单调性改变点
            Image_Change.monotonicity_change_line[1] = Line.Left_Line[Image_Change.monotonicity_change_line[0]];
            Left_Add_Line((int)(Image_Change.monotonicity_change_line[1] * 0.15), LCDH - 1, Image_Change.monotonicity_change_line[1], Image_Change.monotonicity_change_line[0]);
            if ((Island.Island_State == 1) && (Lost.Start_Left < 30)) // 下方当丢线时候进2
            {
                Island.Island_State = 2;
            }
        }
        else if (Island.Island_State == 2) // 下方角点消失，2状态时下方应该是丢线，上面是弧线
        {
            Image_Change.monotonicity_change_line[0] = Monotonicity_Change_Left(20, 10); // 寻找单调性改变点
            Image_Change.monotonicity_change_line[1] = Line.Left_Line[Image_Change.monotonicity_change_line[0]];
            Left_Add_Line((int)(Image_Change.monotonicity_change_line[1] * 0.1), LCDH - 1, Image_Change.monotonicity_change_line[1], Image_Change.monotonicity_change_line[0]);
            if (Island.Island_State == 2 && (Lost.Start_Left>=LCDH-5|| Image_Change.monotonicity_change_line[0] >= 20)) // 当圆弧靠下时候，进3
            {
                Island.Island_State = 3; // 最长白列寻找范围也要改，见camera.c
                Island.Left_Island_Flag = 1;
            }
        }
        else if (Island.Island_State == 3) // 3状态准备进环，寻找上拐点，连线
        {
            if (Island.k != 0)
            {
                K_Draw_Line(Island.k, LCDW - 30, LCDH - 1, 0); // k是刚刚算出来的，静态变量存着
                Longest_White_Column();                        // 刷新边界数据
            }
            else
            {
                Island.Left_Up_Guai[0] = Find_Left_Up_Point(LCDH-1, 5); // 找左上拐点
                Island.Left_Up_Guai[1] = Line.Left_Line[Island.Left_Up_Guai[0]];

//                if (Island.Left_Up_Guai[0] <= 3) // 此处为了防止误判，如果经常从3状态归零，建议修改此处判断条件
//                {
//                    Island.Island_State = 0;
//                    Island.Left_Island_Flag = 0;
//                }

                if (Island.k == 0 && (0 <= Island.Left_Up_Guai[0] && Island.Left_Up_Guai[0] < 50) && (10 < Island.Left_Up_Guai[1] && Island.Left_Up_Guai[1] < LCDW-1)) // 拐点出现在一定范围内，认为是拐点出现
                {
                    Island.island_state_3_up[0] = Island.Left_Up_Guai[0];
                    Island.island_state_3_up[1] = Island.Left_Up_Guai[1];
                    Island.k = (float)((float)(LCDH - Island.island_state_3_up[0]) / (float)(LCDW - 20 - Island.island_state_3_up[1]))/1.1;
                    K_Draw_Line(Island.k, LCDW - 30, LCDH - 1, 0); // 记录下第一次上点出现时位置，针对这个环岛拉一条死线，入环
                    Longest_White_Column();                        // 刷新边界数据
                }
            }
            if ((Island.Island_State == 3) && (abs(Gyroscope.FJ_Angle) >= 70))
            {
                Island.k = 0;            // 斜率清零
                Island.Island_State = 4; // 这一步时候顺便调整了最长白列的搜索范围
                Longest_White_Column();  // 最长白列
            }
        }
        else if (Island.Island_State == 4) // 状态4已经在里面
        {
            if (abs(Gyroscope.FJ_Angle) > 180) // 积分200度以后在打开出环判断
            {
                Image_Change.monotonicity_change_line[0] = Monotonicity_Change_Right(LCDH - 5, 10); // 单调性改变
                Image_Change.monotonicity_change_line[1] = Line.Right_Line[Image_Change.monotonicity_change_line[0]];
                if ((Island.Island_State == 4) && (15 <= Image_Change.monotonicity_change_line[0] && Image_Change.monotonicity_change_line[0] <= 55 && Image_Change.monotonicity_change_line[1] >= 5)) // 单调点靠下，进去5
                {                                                                                                                                                                                       // monotonicity_change_line[1]>=90&&
                    Island.island_state_5_down[0] = LCDH - 1;
                    Island.island_state_5_down[1] = Line.Right_Line[LCDH - 1];
                    Island.k = (float)((float)(LCDH - Image_Change.monotonicity_change_line[0]) / (float)(Island.island_state_5_down[1] - Image_Change.monotonicity_change_line[1]))*1.3;
                    K_Add_Boundry_Right(Island.k, Island.island_state_5_down[1], Island.island_state_5_down[0], 0); // 和状态3一样，记住斜率
                    Island.Island_State = 5;
                }
            }
        }
        else if (Island.Island_State == 5) // 出环
        {
            K_Add_Boundry_Right(Island.k, Island.island_state_5_down[1], Island.island_state_5_down[0], 0);
            if ((Island.Island_State == 5) && (Lost.Start_Right < LCDH - 20)) // 右边先丢线
            {
                Island.Island_State = 6;
            }
        }
        else if (Island.Island_State == 6) // 还在出
        {
            K_Add_Boundry_Right(Island.k, Island.island_state_5_down[1], Island.island_state_5_down[0], 0);
            if ((Island.Island_State == 6) && ((Lost.Start_Right > LCDH - 10) || (abs(Gyroscope.FJ_Angle) >= 320))) // 右边不丢线
            {                                                                                                       //
                Island.k = 0;
                Island.Island_State = 7;
            }
        }
        else if (Island.Island_State == 7) // 基本出去了，在寻找拐点，准备离开环岛状态
        {
            Island.Left_Up_Guai[0] = Find_Left_Up_Point(LCDH - 10, 0); // 获取左上点坐标，坐标点合理去8
            Island.Left_Up_Guai[1] = Line.Left_Line[Island.Left_Up_Guai[0]];
            if ((Island.Island_State == 7) && (Island.Left_Up_Guai[1] <= 90) && (5 <= Island.Left_Up_Guai[0] && Island.Left_Up_Guai[0] <= LCDH - 10)) // 注意这里，对横纵坐标都有要求
            {
                Island.Island_State = 8; // 基本上找到拐点就去8
            }
        }
        else if (Island.Island_State == 8) // 连线，出环最后一步
        {
            Island.Left_Up_Guai[0] = Find_Left_Up_Point(LCDH - 1, 10); // 获取左上点坐标
            Island.Left_Up_Guai[1] = Line.Left_Line[Island.Left_Up_Guai[0]];
            Lengthen_Left_Boundry(Island.Left_Up_Guai[0] - 1, LCDH - 1);
            if ((Island.Island_State == 8) && (Island.Left_Up_Guai[0] >= LCDH - 20 || (Island.Left_Up_Guai[0] < 10 && Lost.Start_Left >= LCDH - 10))) // 当拐点靠下时候，认为出环了，环岛结束
            {                                                                                                                                         // 要么拐点靠下，要么拐点丢了，切下方不丢线，认为环岛结束了
                Gyroscope.FJ_Angle = 0;                                                                                                               // 数据清零
                Island.Island_State = 9;                                                                                                              // 8时候环岛基本结束了，为了防止连续判环，8后会进9，大概几十毫秒后归零，
                Island.Left_Island_Flag = 0;
            }
        }
    }
    else if(Island.Right_Island_Flag==1)
    {
        if(Island.Island_State==1)//1状态下拐点还在，没丢线
        {
            Image_Change.monotonicity_change_line[0]=Monotonicity_Change_Right(20,5);//单调性改变
            Image_Change.monotonicity_change_line[1]=Line.Right_Line[Image_Change.monotonicity_change_line[0]];
            Right_Add_Line((int)(LCDW - 1- (Image_Change.monotonicity_change_line[1]*0.15)),LCDH-1,Image_Change.monotonicity_change_line[1],Image_Change.monotonicity_change_line[0]);
            if(Lost.Start_Right<=30)//右下角先丢线
            {
                Island.Island_State=2;
            }
        }
        else if(Island.Island_State==2)//2状态下方丢线，上方即将出现大弧线
        {
            Image_Change.monotonicity_change_line[0]=Monotonicity_Change_Right(20,5);//单调性改变
            Image_Change.monotonicity_change_line[1]=Line.Right_Line[Image_Change.monotonicity_change_line[0]];
            Right_Add_Line((int)(LCDW - 1- (Image_Change.monotonicity_change_line[1]*0.1)),LCDH-1,Image_Change.monotonicity_change_line[1],Image_Change.monotonicity_change_line[0]);
            if(Island.Island_State==2&&(Lost.Start_Right>=LCDH-5||Image_Change.monotonicity_change_line[0]>=20))//右下角再不丢线进3
            {
                Island.Island_State=3;//下方丢线，说明大弧线已经下来了
                Island.Right_Island_Flag=1;
            }
        }
        else if(Island.Island_State==3)//下面已经出现大弧线，且上方出现角点
        {
            if(Island.k!=0)//将角点与下方连接，画一条死线
            {
                K_Draw_Line(Island.k,30,LCDH-1,0);
                Longest_White_Column();//刷新最长白列
            }
            else
            {
                Island.Right_Up_Guai[0]=Find_Right_Up_Point(LCDH-1,5);//找右上拐点
                Island.Right_Up_Guai[1]=Line.Right_Line[Island.Right_Up_Guai[0]];

//                if(Island.Right_Up_Guai[0]<=3)//这里改过，此处为了防止环岛误判，如果经常出现环岛3归零，请修改此处判断条件
//                {
//                    Island.Island_State=0;
//                    Island.Right_Island_Flag=0;
//                }

                if(Island.k==0&&(15<=Island.Right_Up_Guai[0]&&Island.Right_Up_Guai[0]<50)&&(25<Island.Right_Up_Guai[1]&&Island.Right_Up_Guai[1]<110))//找第一个符合条件的角点，连线
                {
                    Island.island_state_3_up[0]= Island.Right_Up_Guai[0];
                    Island.island_state_3_up[1]= Island.Right_Up_Guai[1];
                    Island.k=(float)((float)(LCDH-Island.island_state_3_up[0])/(float)(20-Island.island_state_3_up[1]))*1.1;
                    K_Draw_Line(Island.k,30,LCDH-1,0);
                    Longest_White_Column();//刷新赛道数据
                }
            }
            if((Island.Island_State==3)&&(abs(Gyroscope.FJ_Angle)>=70))//只依靠陀螺仪积分
            {
                Island.k=0;//斜率清零
                Island.Island_State=4;
                Longest_White_Column();//最长白列
            }//记得去最长白列那边改一下，区分下左右环岛
        }
        else if(Island.Island_State==4)//4状态完全进去环岛了
        {
            if(abs(Gyroscope.FJ_Angle)>180)//环岛积分200度后再打开单调转折判断
            {
                Image_Change.monotonicity_change_line[0]=Monotonicity_Change_Left(LCDH-10,5);//单调性改变
                Image_Change.monotonicity_change_line[1]=Line.Left_Line[Image_Change.monotonicity_change_line[0]];
                if((Island.Island_State==4)&&(25<=Image_Change.monotonicity_change_line[0]&&Image_Change.monotonicity_change_line[0]<=55&&Image_Change.monotonicity_change_line[1]<LCDW-5))//单调点靠下，进去5
                {//monotonicity_change_line[1]<=120&&
                   Island.island_state_5_down[0]=LCDH-1;
                   Island.island_state_5_down[1]=Line.Left_Line[LCDH-1];//抓住第一次出现的斜率，定死
                   Island.k=(float)((float)(LCDH-Image_Change.monotonicity_change_line[0])/(float)(Island.island_state_5_down[1]-Image_Change.monotonicity_change_line[1]))/1.3;
                   K_Add_Boundry_Left(Island.k,Island.island_state_5_down[1],Island.island_state_5_down[0],0);
                   Island.Island_State=5;
                }
            }
        }
        else if(Island.Island_State==5)//准备出环岛
        {
            K_Add_Boundry_Left(Island.k,Island.island_state_5_down[1],Island.island_state_5_down[0],0);
            if(Island.Island_State==5&&Lost.Start_Left<LCDH-20)//左边先丢线
            {
                Island.Island_State=6;
            }
        }
        else if(Island.Island_State==6)//继续出
        {
            K_Add_Boundry_Left(Island.k,Island.island_state_5_down[1],Island.island_state_5_down[0],0);
            if((Island.Island_State==6)&&(Lost.Start_Left>LCDH-10||abs(Gyroscope.FJ_Angle)>=320))//左边先丢线
            {//
                Island.k=0;
                Island.Island_State=7;
            }
        }
        else if(Island.Island_State==7)//基本出环岛，找角点
        {
            Island.Right_Up_Guai[0]=Find_Right_Up_Point(LCDH-10,0);//获取左上点坐标，找到了去8
            Island.Right_Up_Guai[1]=Line.Right_Line[Island.Right_Up_Guai[0]];
            if((Island.Island_State==7)&&((Island.Right_Up_Guai[1]>=5&&(5<=Island.Right_Up_Guai[0]&&Island.Right_Up_Guai[0]<=LCDH-20))))//注意这里，对横纵坐标都有要求，因为赛道不一样，会意外出现拐点
            {//当角点位置合理时，进8
                Island.Island_State=8;
            }
        }
        else if(Island.Island_State==8)//环岛8
        {
            Island.Right_Up_Guai[0]=Find_Right_Up_Point(LCDH-1,10);//获取右上点坐标
            Island.Right_Up_Guai[1]=Line.Right_Line[Island.Right_Up_Guai[0]];
            Lengthen_Right_Boundry(Island.Right_Up_Guai[0]-1,LCDH-1);
            if((Island.Island_State==8)&&(Island.Right_Up_Guai[0]>=LCDH-20||(Island.Right_Up_Guai[0]<10&&Lost.Start_Left>=LCDH-10)))//当拐点靠下时候，认为出环了，环岛结束
            {//角点靠下，或者下端不丢线，认为出环了
                Gyroscope.FJ_Angle=0;
                Island.Island_State=9;
                Island.Right_Island_Flag=0;
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左赛道连续性检测
  @param     起始点，终止点
  @return    连续返回0，不连续返回断线出行数
  Sample     Continuity_Change_Left(int start,int end);
  @note      连续性的阈值设置为5，可更改
-------------------------------------------------------------------------------------------------------------------*/
int Continuity_Change_Left(int start, int end) // 连续性阈值设置为5
{
    int i;
    int t;
    int continuity_change_flag = 0;
    if (Lost.Left_Num >= 0.9 * LCDH) // 大部分都丢线，没必要判断了
        return 1;
    if (LonWCol.Search_Stop_Line <= 5) // 搜所截止行很矮
        return 1;
    if (start >= LCDH - 1 - 5) // 数组越界保护
        start = LCDH - 1 - 5;
    if (end <= 5)
        end = 5;
    if (start < end) // 都是从下往上计算的，反了就互换一下
    {
        t = start;
        start = end;
        end = t;
    }

    for (i = start; i >= end; i--)
    {
        if (abs(Line.Left_Line[i] - Line.Left_Line[i - 1]) >= CONTINUITY_THRESHOLDS) // 连续判断阈值是5,可更改
        {
            continuity_change_flag = i;
            break;
        }
    }
    return continuity_change_flag;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右赛道连续性检测
  @param     起始点，终止点
  @return    连续返回0，不连续返回断线出行数
  Sample     continuity_change_flag=Continuity_Change_Right(int start,int end)
  @note      连续性的阈值设置为5，可更改
-------------------------------------------------------------------------------------------------------------------*/
int Continuity_Change_Right(int start, int end)
{
    int i;
    int t;
    int continuity_change_flag = 0;
    if (Lost.Right_Num >= 0.9 * LCDH) // 大部分都丢线，没必要判断了
        return 1;
    if (start >= LCDH - 5) // 数组越界保护
        start = LCDH - 5;
    if (end <= 5)
        end = 5;
    if (start < end) // 都是从下往上计算的，反了就互换一下
    {
        t = start;
        start = end;
        end = t;
    }

    for (i = start; i >= end; i--)
    {
        if (abs(Line.Right_Line[i] - Line.Right_Line[i - 1]) >= CONTINUITY_THRESHOLDS) // 连续性阈值是5，可更改
        {
            continuity_change_flag = i;
            break;
        }
    }
    return continuity_change_flag;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     单调性突变检测
  @param     起始点，终止行
  @return    点所在的行数，找不到返回0
  Sample     Find_Right_Up_Point(int start,int end);
  @note      前5后5它最大（最小），那他就是角点
-------------------------------------------------------------------------------------------------------------------*/
int Monotonicity_Change_Left(int start, int end) // 单调性改变，返回值是单调性改变点所在的行数
{
    int i;
    int monotonicity_change_line = 0;
    if (Lost.Left_Num >= 0.95 * LCDH) // 大部分都丢线，没有单调性判断的意义
        return monotonicity_change_line;
    if (start >= LCDH - 1 - 5) // 数组越界保护，在判断第i个点时
        start = LCDH - 1 - 5;  // 要访问它前后5个点，数组两头的点要不能作为起点终点
    if (end <= 5)
        end = 5;
    if (start <= end) // 递减计算，入口反了，直接返回0
        return monotonicity_change_line;
    for (i = start; i >= end; i--) // 会读取前5后5数据，所以前面对输入范围有要求
    {
        if (Line.Left_Line_Display[i] == Line.Left_Line_Display[i + 5] && Line.Left_Line_Display[i] == Line.Left_Line_Display[i - 5] &&
            Line.Left_Line_Display[i] == Line.Left_Line_Display[i + 4] && Line.Left_Line_Display[i] == Line.Left_Line_Display[i - 4] &&
            Line.Left_Line_Display[i] == Line.Left_Line_Display[i + 3] && Line.Left_Line_Display[i] == Line.Left_Line_Display[i - 3] &&
            Line.Left_Line_Display[i] == Line.Left_Line_Display[i + 2] && Line.Left_Line_Display[i] == Line.Left_Line_Display[i - 2] &&
            Line.Left_Line_Display[i] == Line.Left_Line_Display[i + 1] && Line.Left_Line_Display[i] == Line.Left_Line_Display[i - 1])
        { // 一堆数据一样，显然不能作为单调转折点
            continue;
        }
        else if (Line.Left_Line_Display[i] >= Line.Left_Line_Display[i + 5] && Line.Left_Line_Display[i] >= Line.Left_Line_Display[i - 5] &&
                 Line.Left_Line_Display[i] >= Line.Left_Line_Display[i + 4] && Line.Left_Line_Display[i] >= Line.Left_Line_Display[i - 4] &&
                 Line.Left_Line_Display[i] >= Line.Left_Line_Display[i + 3] && Line.Left_Line_Display[i] >= Line.Left_Line_Display[i - 3] &&
                 Line.Left_Line_Display[i] >= Line.Left_Line_Display[i + 2] && Line.Left_Line_Display[i] >= Line.Left_Line_Display[i - 2] &&
                 Line.Left_Line_Display[i] >= Line.Left_Line_Display[i + 1] && Line.Left_Line_Display[i] >= Line.Left_Line_Display[i - 1])
        { // 就很暴力，这个数据是在前5，后5中最大的（可以取等），那就是单调突变点
            monotonicity_change_line = i;
            break;
        }
    }
    return monotonicity_change_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     单调性突变检测
  @param     起始点，终止行
  @return    点所在的行数，找不到返回0
  Sample     Find_Right_Up_Point(int start,int end);
  @note      前5后5它最大（最小），那他就是角点
-------------------------------------------------------------------------------------------------------------------*/
int Monotonicity_Change_Right(int start, int end) // 单调性改变，返回值是单调性改变点所在的行数
{
    int i;
    int monotonicity_change_line = 0;

    if (Lost.Right_Num >= 0.95 * LCDH) // 大部分都丢线，没有单调性判断的意义
        return monotonicity_change_line;
    if (start >= LCDH - 1 - 5) // 数组越界保护
        start = LCDH - 1 - 5;
    if (end <= 5)
        end = 5;
    if (start <= end)
        return monotonicity_change_line;
    for (i = start; i >= end; i--) // 会读取前5后5数据，所以前面对输入范围有要求
    {
        if (Line.Right_Line_Display[i] == Line.Right_Line_Display[i + 5] && Line.Right_Line_Display[i] == Line.Right_Line_Display[i - 5] &&
            Line.Right_Line_Display[i] == Line.Right_Line_Display[i + 4] && Line.Right_Line_Display[i] == Line.Right_Line_Display[i - 4] &&
            Line.Right_Line_Display[i] == Line.Right_Line_Display[i + 3] && Line.Right_Line_Display[i] == Line.Right_Line_Display[i - 3] &&
            Line.Right_Line_Display[i] == Line.Right_Line_Display[i + 2] && Line.Right_Line_Display[i] == Line.Right_Line_Display[i - 2] &&
            Line.Right_Line_Display[i] == Line.Right_Line_Display[i + 1] && Line.Right_Line_Display[i] == Line.Right_Line_Display[i - 1])
        { // 一堆数据一样，显然不能作为单调转折点
            continue;
        }
        else if (Line.Right_Line_Display[i] <= Line.Right_Line_Display[i + 5] && Line.Right_Line_Display[i] <= Line.Right_Line_Display[i - 5] &&
                 Line.Right_Line_Display[i] <= Line.Right_Line_Display[i + 4] && Line.Right_Line_Display[i] <= Line.Right_Line_Display[i - 4] &&
                 Line.Right_Line_Display[i] <= Line.Right_Line_Display[i + 3] && Line.Right_Line_Display[i] <= Line.Right_Line_Display[i - 3] &&
                 Line.Right_Line_Display[i] <= Line.Right_Line_Display[i + 2] && Line.Right_Line_Display[i] <= Line.Right_Line_Display[i - 2] &&
                 Line.Right_Line_Display[i] <= Line.Right_Line_Display[i + 1] && Line.Right_Line_Display[i] <= Line.Right_Line_Display[i - 1])
        { // 就很暴力，这个数据是在前5，后5中最大的，那就是单调突变点
            monotonicity_change_line = i;
            break;
        }
    }
    return monotonicity_change_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左下角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Left_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Left_Down_Point(int start, int end) // 找四个角点，返回值是角点所在的行数
{
    int i, t;
    int left_down_line = 0;
    if (Lost.Left_Num >= 0.9 * LCDH) // 大部分都丢线，没有拐点判断的意义
        return left_down_line;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (start >= LCDH - 1 - 5) // 下面5行数据不稳定，不能作为边界点来判断，舍弃
        start = LCDH - 1 - 5;
    if (end <= LCDH - LonWCol.Search_Stop_Line)
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 5)
        end = 5;
    for (i = start; i >= end; i--)
    {
        if (left_down_line == 0 &&                                                  // 只找第一个符合条件的点
            abs(Line.Left_Line[i] - Line.Left_Line[i + 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Left_Line[i + 1] - Line.Left_Line[i + 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Left_Line[i + 2] - Line.Left_Line[i + 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Left_Line[i] - Line.Left_Line[i - 2]) >= FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Left_Line[i] - Line.Left_Line[i - 3]) >= FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Left_Line[i] - Line.Left_Line[i - 4]) >= FOOT_THRESHOLD_LINE3_MIN)
        {
            left_down_line = i; // 获取行数即可
            break;
        }
    }
    return left_down_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左上角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Left_Up_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Left_Up_Point(int start, int end) // 找四个角点，返回值是角点所在的行数
{
    int i, t;
    int left_up_line = 0;
    if (Lost.Left_Num >= 0.9 * LCDH) // 大部分都丢线，没有拐点判断的意义
        return left_up_line;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (end <= LCDH - LonWCol.Search_Stop_Line) // 搜索截止行往上的全都不判
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 5) // 及时最长白列非常长，也要舍弃部分点，防止数组越界
        end = 5;
    if (start >= LCDH - 1 - 5)
        start = LCDH - 1 - 5;
    for (i = start; i >= end; i--)
    {
        if (left_up_line == 0 && // 只找第一个符合条件的点
            abs(Line.Left_Line[i] - Line.Left_Line[i - 1]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Left_Line[i - 1] - Line.Left_Line[i - 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Left_Line[i - 2] - Line.Left_Line[i - 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Left_Line[i] - Line.Left_Line[i + 2]) >= FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Left_Line[i] - Line.Left_Line[i + 3]) >= FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Left_Line[i] - Line.Left_Line[i + 4]) >= FOOT_THRESHOLD_LINE3_MIN)
        {
            left_up_line = i; // 获取行数即可
            break;
        }
    }
    return left_up_line; // 如果是LCDH-1，说明没有这么个拐点
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右下角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Right_Down_Point(int start, int end) // 找四个角点，返回值是角点所在的行数
{
    int i, t;
    int right_down_line = 0;
    if (Lost.Right_Num >= 0.9 * LCDH) // 大部分都丢线，没有拐点判断的意义
        return right_down_line;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (start >= LCDH - 1 - 5) // 下面5行数据不稳定，不能作为边界点来判断，舍弃
        start = LCDH - 1 - 5;
    if (end <= LCDH - LonWCol.Search_Stop_Line)
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 5)
        end = 5;
    for (i = start; i >= end; i--)
    {
        if (right_down_line == 0 &&                                                   // 只找第一个符合条件的点
            abs(Line.Right_Line[i] - Line.Right_Line[i + 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Right_Line[i + 1] - Line.Right_Line[i + 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Right_Line[i + 2] - Line.Right_Line[i + 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Right_Line[i] - Line.Right_Line[i - 2]) <= -FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Right_Line[i] - Line.Right_Line[i - 3]) <= -FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Right_Line[i] - Line.Right_Line[i - 4]) <= -FOOT_THRESHOLD_LINE3_MIN)
        {
            right_down_line = i; // 获取行数即可
            break;
        }
    }
    return right_down_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右上角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Up_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Right_Up_Point(int start, int end) // 找四个角点，返回值是角点所在的行数
{
    int i, t;
    int right_up_line = 0;
    if (Lost.Right_Num >= 0.9 * LCDH) // 大部分都丢线，没有拐点判断的意义
        return right_up_line;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (end <= LCDH - LonWCol.Search_Stop_Line) // 搜索截止行往上的全都不判
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 5) // 及时最长白列非常长，也要舍弃部分点，防止数组越界
        end = 5;
    if (start >= LCDH - 1 - 5)
        start = LCDH - 1 - 5;
    for (i = start; i >= end; i--)
    {
        if (right_up_line == 0 &&                                                     // 只找第一个符合条件的点
            abs(Line.Right_Line[i] - Line.Right_Line[i - 1]) <= FOOT_THRESHOLD_MAX && // 下面两行位置差不多
            abs(Line.Right_Line[i - 1] - Line.Right_Line[i - 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Right_Line[i - 2] - Line.Right_Line[i - 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Right_Line[i] - Line.Right_Line[i + 2]) <= -FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Right_Line[i] - Line.Right_Line[i + 3]) <= -FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Right_Line[i] - Line.Right_Line[i + 4]) <= -FOOT_THRESHOLD_LINE3_MIN)
        {
            right_up_line = i; // 获取行数即可
            break;
        }
    }
    return right_up_line;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     通过斜率，定点补线--
  @param     k       输入斜率
             startY  输入起始点纵坐标
             endY    结束点纵坐标
  @return    null
  Sample     K_Add_Boundry_Left(float k,int startY,int endY);
  @note      补得线直接贴在边线上
-------------------------------------------------------------------------------------------------------------------*/
void K_Add_Boundry_Left(float k, int startX, int startY, int endY)
{
    int i, t;
    if (startY >= LCDH - 1)
        startY = LCDH - 1;
    else if (startY <= 0)
        startY = 0;
    if (endY >= LCDH - 1)
        endY = LCDH - 1;
    else if (endY <= 0)
        endY = 0;
    if (startY < endY) //--操作，start需要大
    {
        t = startY;
        startY = endY;
        endY = t;
    }
    for (i = startY; i >= endY; i--)
    {
        Line.Left_Line_Display[i] = (int)((i - startY) / k + startX); //(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1
        if (Line.Left_Line_Display[i] >= LCDW - 1)
        {
            Line.Left_Line_Display[i] = LCDW - 1;
        }
        else if (Line.Left_Line_Display[i] <= 0)
        {
            Line.Left_Line_Display[i] = 0;
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     通过斜率，定点补线
  @param     k       输入斜率
             startY  输入起始点纵坐标
             endY    结束点纵坐标
  @return    null    直接补边线
  Sample     K_Add_Boundry_Right(float k,int startY,int endY);
  @note      补得线直接贴在边线上
-------------------------------------------------------------------------------------------------------------------*/
void K_Add_Boundry_Right(float k, int startX, int startY, int endY)
{
    int i, t;
    if (startY >= LCDH - 1)
        startY = LCDH - 1;
    else if (startY <= 0)
        startY = 0;
    if (endY >= LCDH - 1)
        endY = LCDH - 1;
    else if (endY <= 0)
        endY = 0;
    if (startY < endY)
    {
        t = startY;
        startY = endY;
        endY = t;
    }
    for (i = startY; i >= endY; i--)
    {
        Line.Right_Line_Display[i] = (int)((i - startY) / k + startX); //(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1
        if (Line.Right_Line_Display[i] >= LCDW - 1)
        {
            Line.Right_Line_Display[i] = LCDW - 1;
        }
        else if (Line.Right_Line_Display[i] <= 0)
        {
            Line.Right_Line_Display[i] = 0;
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     根据斜率划线
  @param     输入斜率，定点，画一条黑线
  @return    null
  Sample     K_Draw_Line(k, 20,LCDH-1 ,0)
  @note      补的就是一条线，需要重新扫线
-------------------------------------------------------------------------------------------------------------------*/
void K_Draw_Line(float k, int startX, int startY, int endY)
{
    int endX = 0;

    if (startX >= LCDW - 1) // 限幅处理
        startX = LCDW - 1;
    else if (startX <= 0)
        startX = 0;
    if (startY >= LCDH - 1)
        startY = LCDH - 1;
    else if (startY <= 0)
        startY = 0;
    if (endY >= LCDH - 1)
        endY = LCDH - 1;
    else if (endY <= 0)
        endY = 0;
    endX = (int)((endY - startY) / k + startX); //(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1
    Draw_Line(startX, startY, endX, endY);
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     获取平均赛宽
  @param     int start_line,int end_line，起始行，中止行
  @return    这几行赛宽平均值
  Sample     road_wide=Get_Road_Wide(68,69);
  @note      ++运算，向下寻找，算出平均赛宽
-------------------------------------------------------------------------------------------------------------------*/
int Get_Road_Wide(int start_line, int end_line)
{
    if (start_line >= LCDH - 1)
        start_line = LCDH - 1;
    else if (start_line <= 0)
        start_line = 0;
    if (end_line >= LCDH - 1)
        end_line = LCDH - 1;
    else if (end_line <= 0)
        end_line = 0;
    int i = 0, t = 0;
    int road_wide = 0;
    if (start_line > end_line) //++访问，坐标反了互换
    {
        t = start_line;
        start_line = end_line;
        end_line = t;
    }
    for (i = start_line; i <= end_line; i++)
    {
        road_wide += Line.Right_Line_Display[i] - Line.Left_Line_Display[i];
    }
    road_wide = road_wide / (end_line - start_line + 1); // 平均赛宽
    return road_wide;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     获取左赛道边界斜率
  @param     int start_line,int end_line，起始行，中止行
  @return    两点之间的斜率
  Sample     k=Get_Left_K(68,69);
  @note      两点之间得出斜率，默认第一个参数小，第二个参数大
-------------------------------------------------------------------------------------------------------------------*/
float Get_Left_K(int start_line, int end_line)
{
    if (start_line >= LCDH - 1)
        start_line = LCDH - 1;
    else if (start_line <= 0)
        start_line = 0;
    if (end_line >= LCDH - 1)
        end_line = LCDH - 1;
    else if (end_line <= 0)
        end_line = 0;
    float k = 0;
    int t = 0;
    if (start_line > end_line) //++访问，坐标反了互换
    {
        t = start_line;
        start_line = end_line;
        end_line = t;
    }
    k = (float)(((float)Line.Left_Line[start_line] - (float)Line.Left_Line[end_line]) / (end_line - start_line + 1));
    return k;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     获取右赛道边界斜率
  @param     int start_line,int end_line，起始行，中止行
  @return    两点之间的斜率
  Sample     k=Get_Right_K(68,69);
  @note      两点之间得出斜率，默认第一个参数小，第二个参数大
-------------------------------------------------------------------------------------------------------------------*/
float Get_Right_K(int start_line, int end_line)
{
    if (start_line >= LCDH - 1)
        start_line = LCDH - 1;
    else if (start_line <= 0)
        start_line = 0;
    if (end_line >= LCDH - 1)
        end_line = LCDH - 1;
    else if (end_line <= 0)
        end_line = 0;
    float k = 0;
    int t = 0;
    if (start_line > end_line) //++访问，坐标反了互换
    {
        t = start_line;
        start_line = end_line;
        end_line = t;
    }
    k = (float)(((float)Line.Right_Line[start_line] - (float)Line.Right_Line[end_line]) / (end_line - start_line + 1));
    return k;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     画线
  @param     输入起始点，终点坐标，补一条宽度为2的黑线
  @return    null
  Sample     Draw_Line(0, 0,LCDW-1,LCDH-1);
             Draw_Line(LCDW-1, 0,0,LCDH-1);
                                    画一个大×
  @note     补的就是一条线，需要重新扫线
-------------------------------------------------------------------------------------------------------------------*/
void Draw_Line(int startX, int startY, int endX, int endY)
{
    int i, x, y;
    int start = 0, end = 0;
    if (startX >= LCDW - 1) // 限幅处理
        startX = LCDW - 1;
    else if (startX <= 0)
        startX = 0;
    if (startY >= LCDH - 1)
        startY = LCDH - 1;
    else if (startY <= 0)
        startY = 0;
    if (endX >= LCDW - 1)
        endX = LCDW - 1;
    else if (endX <= 0)
        endX = 0;
    if (endY >= LCDH - 1)
        endY = LCDH - 1;
    else if (endY <= 0)
        endY = 0;
    if (startX == endX) // 一条竖线
    {
        if (startY > endY) // 互换
        {
            start = endY;
            end = startY;
        }
        for (i = start; i <= end; i++)
        {
            if (i <= 1)
                i = 1;
            TFT18_Bin_data[i][startX] = IMG_BLACK;
            TFT18_Bin_data[i - 1][startX] = IMG_BLACK;
        }
    }
    else if (startY == endY) // 补一条横线
    {
        if (startX > endX) // 互换
        {
            start = endX;
            end = startX;
        }
        for (i = start; i <= end; i++)
        {
            if (startY <= 1)
                startY = 1;
            TFT18_Bin_data[startY][i] = IMG_BLACK;
            TFT18_Bin_data[startY - 1][i] = IMG_BLACK;
        }
    }
    else // 上面两个是水平，竖直特殊情况，下面是常见情况
    {
        if (startY > endY) // 起始点矫正
        {
            start = endY;
            end = startY;
        }
        else
        {
            start = startY;
            end = endY;
        }
        for (i = start; i <= end; i++) // 纵向补线，保证每一行都有黑点
        {
            x = (int)(startX + (endX - startX) * (i - startY) / (endY - startY)); // 两点式变形
            if (x >= LCDW - 1)
                x = LCDW - 1;
            else if (x <= 1)
                x = 1;
            TFT18_Bin_data[i][x] = IMG_BLACK;
            TFT18_Bin_data[i][x - 1] = IMG_BLACK;
        }
        if (startX > endX)
        {
            start = endX;
            end = startX;
        }
        else
        {
            start = startX;
            end = endX;
        }
        for (i = start; i <= end; i++) // 横向补线，保证每一列都有黑点
        {

            y = (int)(startY + (endY - startY) * (i - startX) / (endX - startX)); // 两点式变形
            if (y >= LCDH - 1)
                y = LCDH - 1;
            else if (y <= 0)
                y = 0;
            TFT18_Bin_data[y][i] = IMG_BLACK;
        }
    }
}
